#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>


#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>

#include <pthread.h>

#include "include/cal.h"
#include "include/obd_function.h"
#include "include/obd_debug.h"
#include "include/obd_uart.h"
#include "include/vehicle.h"
#include "include/filesystem.h"
#include "include/stn.h"
#include "include/sysutils.h"
#include "librtc/rtc_lib.h"
#include "include/sound.h"
#include "include/vehiclestatus.h"

#define FALSE 0
#define TRUE  1

#define RX_BUF_NAME	buf
#define OBD_CMD_DELAY 10000
#define OBD_CMD_DELAY1 200000
#define WIFI_INITIAL_TIMEOUT 		120
#define WIFI_ENGINE_IDEL_TIMEOUT	60
#define WIFI_ENGINE_START_TIMEOUT	0
#define WIFI_ENGINE_STOP_TIMEOUT	120

#define WIFI_ON		1
#define WIFI_OFF	0


#define SYSTEM_INITIAL		0
#define SYSTEM_ENGINE_IDEL	1
#define SYSTEM_ENGINE_START	2
#define SYSTEM_ENGINE_STOP	3

#define INITIAL		0
#define CHECKCONFIG	1
#define CONNFAILED	2
#define NORMAL			3


#define PMCON	0xFFF00028	/* Power Managerment Regisiter of ARM */
#define PMCON_IDLE	0x00000001
#define PMCON_PD		0x00000002
#define PMCON_MIDLE	0x00000004


#define IRQWAKECON	0xFFF00020
#define IRQWAKEUPEN0		0x01
#define IRQWAKEUPEN1		0x02
#define IRQWAKEUPEN2		0x04
#define IRQWAKEUPEN3		0x08

#define IRQWAKEUPPOL0	0x10
#define IRQWAKEUPPOL1	0x20
#define IRQWAKEUPPOL2	0x40
#define IRQWAKEUPPOL3	0x80

#define IRQWAKEFLAG	0xFFF00024
#define IRQWAKEFLAG0		0x01
#define IRQWAKEFLAG1		0x02
#define IRQWAKEFLAG2		0x04
#define IRQWAKEFLAG3		0x08

#define EBICTRLREG	0xFFF01000
#define EBICTRL_CLKEN	0x00010000
#define EBICTRL_REFMOD	0x00020000
#define EBICTRL_REFEN	0x00040000

#define outp(port,value)	*((volatile byte *)(port)) = value
#define outpdw(port,value)	*((volatile short *)(port)) = value
#define outpw(port, value) *((volatile unsigned int *)(port)) = value

#define inp(port)		*((volatile byte *)(port))
#define inpdw(port)	*((volatile short *)(port))
#define inpw(port)	*((volatile unsigned int *)(port))


#define RTC_TICK_ON		1
#define RTC_TICK_OFF		0

#define SOUND_WIFION_ALREADY 1
#define SOUND_WIFION_FIRST 1

#define SOUND_WIFIOFF_ALRAEDY 0
#define SOUND_WIFIOFF_FIRST 1

#define SOUND_SUSPEND_ALREADY 0
#define SOUND_SUSPEND_FIRST	1

#define SOUND_RESUME_ALREADY 0
#define SOUND_RESUME_FIRST 1


struct vehicle_status vs;
struct vehicle_cmd vcmd;
struct fuel_rate_para frp;
struct rtc_time tm;
unsigned int timer = 0;
unsigned int date_flag = 0;

unsigned char GPS_POS[13];
unsigned int DATE_TIME[6];
unsigned char RTC_TIME[6];

unsigned char cBAT = 0;
unsigned char wBAT = 0;

int suspend_sound_flag = 0;
int wifion_sound_flag = 0;
int wifioff_sound_flag = 0;


int STN_sleep = 0;	

int Wifi_timer = 0;		// Wifi on/off  计时器
int Wifi_status = WIFI_ON;		// Wifi on/off 状态  0：Off, 1: On
int System_flag = 0;		// 系统状态标志： 
								//	0： 初始化状态 
								//	1： 引擎闲置状态
								//	2：	引擎启动状态
								//	3： 引擎关闭状态

int EngineStatus = 0;
int force_stop = 0;
/* declare variables for Vehicle Information */
struct vehicle_information vi_real;

/* Function Pre-declare */
int Wifi_control(int flag);

//-----------------------------------------------------------------------------------------------------------------------------------------------------------
void quit(int x, int fd)
{
	if(fd > 0) close(fd);
	exit(x);
}


/*获取车辆信息函数*/
void GetDataFromVehicle(int fd, unsigned char * data, 			\
									struct vehicle_information * vi_real,		\
									unsigned char alpha)
{
	unsigned char * pdata = malloc(CONFIG_LENGTH);
	//struct vehicle_information *vi_real = malloc(sizeof(struct vehicle_information));
	
	unsigned char * cptr;
	unsigned char rxbuf[rxbuf_size];
	int ret;

	pdata = data;
	//vi_real = vi; 
	memset(rxbuf, 0, rxbuf_size);

	vi_real->protocol_num = STN_Check_Protocol(fd, rxbuf, alpha, pdata);
	printf("Vehicle Protocol Number : %02X\n", vi_real->protocol_num);

	if( vi_real->protocol_num > 0xA){
		printf("Protocol of this Vehicle is not supported!\n");
		/* add Voice Alarm Here */
		
		exit(1);
	}

		cptr = pdata + 1;
		memcpy(vi_real->protocol_name, cptr, 15);
		printf("Vehicle Protocl Name: %s\n", vi_real->protocol_name);
		
		if(vi_real->protocol_num <= 5){	
			// KWP Protocal Turn Header On
			vi_real->protocol_type = PROTOCOL_TYPE_KWP;
			while(1){
				memset(rxbuf, 0, rxbuf_size);
				tcflush(fd, TCIOFLUSH);	
				ret = AT_Set_Header(fd, rxbuf, 1);
				//printf("ret:%d \n", ret);
				if( alpha ) {
					processSTNreturn(rxbuf, ret, SET_HEADER, 1);
				}
				if(ret == 2 && strncmp(rxbuf, "OK", 2) == 0) break;
				usleep(10000);
			}
			if(alpha) printf("Turn Header On\n");
			//AT_Set_HeaderValue_B(fd, rxbuf, 0xE4, 0x10, 0xF1);
			//if(alpha) printf("Set ECU Header E4 10 F1\n");
			AT_ISO_Set_WakeupInterval(fd, rxbuf, 0xFF);

			vcmd.CheckConnect = KWP_Check_Connect;
			vcmd.GetPIDSupport = KWP_Get_PID_Support;	
			vcmd.GetVIN =	KWP_Get_VIN;	
			vcmd.GetCALID = KWP_Get_CALID;
			vcmd.GetRPM = KWP_Get_EngineRPM;
			vcmd.GetEFR = KWP_Get_EngineEFR;
			vcmd.GetMAP = KWP_Get_EngineMAP;
			vcmd.GetFUELLV = KWP_Get_Fuel_LV;
			vcmd.GetIAT = KWP_Get_EngineIAT;
			vcmd.GetMAF = KWP_Get_EngineMAF;
			vcmd.GetSTFT = KWP_Get_EngineSTFT;
			vcmd.GetLTFT = KWP_Get_EngineLTFT;
			vcmd.GetSPEED = KWP_Get_SPEED;
			vcmd.GetENGINELOAD = KWP_Get_ENGINE_LOAD;
			vcmd.GetCOOLTEMP = KWP_Get_COOL_TEMP;
			vcmd.GetPADPOS	= KWP_Get_PAD_POS;
			vcmd.GetDIST21 = KWP_Get_DIST21;
			vcmd.GetDIST31 = KWP_Get_DIST31;
			

			if(alpha) printf("Vehicle Use KWP Command Type\n");
		}
		else{
			// CAN Protocal Turn Header On
			vi_real->protocol_type = PROTOCOL_TYPE_CAN;
			while(1){
				memset(rxbuf, 0, rxbuf_size);
				tcflush(fd, TCIOFLUSH);	
				ret = AT_Set_Header(fd, rxbuf, 1);
				//printf("ret:%d \n", ret);
				if( alpha ) {
					processSTNreturn(rxbuf, ret, SET_HEADER, 1);
				}
				if(ret == 2 && strncmp(rxbuf, "OK", 2) == 0) break;
				usleep(10000);
			}
			if(alpha) printf("Turn Header On\n");
			while(CAN_Judge_ECUS(fd,rxbuf, 1) == 0)
				sleep(1);
			vcmd.CheckConnect = CAN_Check_Connect;
			vcmd.GetPIDSupport = CAN_Get_PID_Support;
			vcmd.GetVIN = CAN_Get_VIN;
			vcmd.GetCALID = CAN_Get_CALID;
			vcmd.GetRPM = CAN_Get_EngineRPM;
			vcmd.GetEFR = CAN_Get_EngineEFR;
			vcmd.GetMAP = CAN_Get_EngineMAP;
			vcmd.GetFUELLV = CAN_Get_Fuel_LV;
			vcmd.GetIAT = CAN_Get_EngineIAT;
			vcmd.GetMAF = CAN_Get_EngineMAF;
			vcmd.GetSTFT = CAN_Get_EngineSTFT;
			vcmd.GetLTFT = CAN_Get_EngineLTFT;
			vcmd.GetSPEED = CAN_Get_SPEED;
			vcmd.GetENGINELOAD = CAN_Get_ENGINE_LOAD;
			vcmd.GetCOOLTEMP = CAN_Get_COOL_TEMP;
			vcmd.GetPADPOS = CAN_Get_PAD_POS;
			vcmd.GetDIST21 = CAN_Get_DIST21;
			vcmd.GetDIST31 = CAN_Get_DIST31;
			vcmd.GetMultiMAP = CAN_Multi_MAP;
			vcmd.GetMultiMAF = CAN_Multi_MAF;
			vcmd.GetMultiEFR = CAN_Multi_EFR;
			vcmd.GetMultiDriveBehavior = CAN_GetMultiDriveBehavior;
			
			if(alpha) printf("Vehicle Use CAN Command Type\n");
		}


		// GET SN of STN1110
		while(1){
			memset(rxbuf, 0, rxbuf_size);
			tcflush(fd, TCIOFLUSH);	
			ret = ST_Get_SerialNum(fd, rxbuf);
			if(ret == 12 ) break;
			usleep(200000);
		}
		cptr = rxbuf + 4;
		memcpy(vi_real->SN, cptr, SN_LEN);
		memcpy(pdata+CONFIG_SN_ADDR, cptr, SN_LEN);
		if(alpha) printf("SN: %s\n", vi_real->SN);		

		vcmd.GetPIDSupport(fd, rxbuf, alpha, pdata);
		
		cptr = pdata + CONFIG_PID00_ADDR;
		memcpy(vi_real->PID00, cptr, 4);
		cptr += 4;
		memcpy(vi_real->PID20, cptr, 4);
		cptr += 4;
		memcpy(vi_real->PID40, cptr, 4);
		cptr += 4;
		memcpy(vi_real->PID90, cptr, 4);
		cptr += 4;
		vi_real->gas_cal = *(cptr++);
		vi_real->fuel_cal[0] = *(cptr++);
		vi_real->fuel_cal[1] = *(cptr++);
		vi_real->POI = *(cptr++);
		vi_real->dist_cal[0] = *(cptr++);
		vi_real->dist_cal[1] = *cptr;

		/* Check Vehicle Infomation */	
		//printf("VI_REAL->PID90: %02X\n", vi_real->PID90[0]);		
		if(vi_real->PID90[0] > 0){
			if( (vi_real->PID90[0] & 0xC0) > 0 ){
				/* Get VIN of Vehicle */
				vcmd.GetVIN(fd, rxbuf, alpha, pdata);
				cptr = pdata + CONFIG_VIN_ADDR;
				memcpy(vi_real->VIN, cptr, VIN_LEN);
				//if(alpha) printf("VIN of Vehicle : %s\n", vi_real->VIN);
			}else
				if(alpha) printf("VIN of Vehicle: NOT SUPPORTED! [%02X]\n",vi_real->PID90[0]);
			
			if( (vi_real->PID90[0] & 0x30) > 0 ){
				/* Get CALID of Vehicle */
				vcmd.GetCALID(fd, rxbuf, alpha, pdata);
				cptr = pdata + CONFIG_CALID_ADDR;
				memcpy(vi_real->CALID, cptr, CALID_LEN);
				//if(alpha) printf("CALID of Vehicle : %s\n", vi_real->CALID);
			}else
				if(alpha) printf("CALID of Vehicle: NOT SUPPORTED!\n");
		}
		
		/* Calculation for CID */
		Cal_Vehicle_CID(pdata);
		cptr = pdata + CONFIG_CID_ADDR;
		memcpy(vi_real->CID, cptr, CID_LEN);

		/* CheckSum for Config File */	
		Add_Config_CheckSum(pdata);

}

/*引擎启动处理函数*/
void EngineStart(void)
{
	
	init_Trip(&tstatus);
	init_OBD_DATA(&odata);
	init_CAL(&cdata, &mdata, &sdata);

	/* record Battery Voltage */
	updateSummaryBat(&sdata, wBAT);
	get_time(RTC_TIME);

	updateMinTime(&mdata, RTC_TIME);

	updateSummaryTimeStart(&sdata, RTC_TIME);
	//updateSummaryGPSStart(&sdata, GPS_POS);

	updateSummaryBat(&sdata, wBAT);
	EngineStatus = 1;
	startTripDataRecord(&tstatus);
}

/*引擎停止处理函数*/
void EngineStop(void)
{
	EngineStatus = 0;
	get_time(RTC_TIME);
	updateSummaryTimeEnd(&sdata, RTC_TIME);
	/* Send Message to Thread of Calculation to Finished Trip */
	odata.data[0] = -1;
	saveCarData(&cdata, odata.data);
}


/*RTC 每1秒触发的执行函数*/
void * RTC_1S(void * data)
{
	unsigned char datetime[TIME_FILE_MAX_LEN];
	FILE * runin;
	if((runin = fopen(RUN_FILE, "r"))  != NULL)
	{
		fclose(runin);
		remove(RUN_FILE);
		}

	timer++;
	
	if(timer == 7200) timer = 0;

	
	/* 	Cal Fuelrate */
	if(EngineStatus == 1){
		if(vi_real.gas_cal == 0x5E || vi_real.fuel_cal[0] != 0x20){
			vcmd.GetFR(&frp);
			updateOBDDATA(&odata, frp.fuelrate, OBD_DATA_FR);
			//修改日期： 2013/10/08
			//修改人： Ray Goo
			//修改原因： 由于在分钟的运算中会计算百公里油耗，所以在秒级的运算中去除			
			//updateOBDDATA(&odata, frp.fuelrate100, OBD_DATA_FR100);
			saveCarData(&cdata, odata.data);
		}
		//跟新油箱存量
		updateMinFuellv(&mdata, vs.Fuel_LV);
		//更新Slice数据中的GPS信息
		//updateMinGPS(&mdata, GPS_POS);
		//获取RTC时间
		get_time(RTC_TIME);
		//更新Slice数据中的时间
		updateMinTime(&mdata, RTC_TIME);
	}
	else
	{
		/* 如果WIFI 功能开启 */
		if(Wifi_status == WIFI_ON)
		{
			if(GetDateTimeFromFile(DATE_TIME) != TIME_FILE_NOT_EXIST)
			{
				set_time(DATE_TIME);
				Sound_status = SOUND_ON;
				saveSoundList(&sl, SOUND_TIMESYNC_INDEX, Sound_status);
				}
			}
	}
	/* Wifi On/off Control */
	switch(System_flag){
		case SYSTEM_INITIAL:
				if(Wifi_timer >= WIFI_INITIAL_TIMEOUT){
					if(Wifi_status == WIFI_ON)
						Wifi_control(WIFI_OFF);
				}
				else
					Wifi_timer++;
				break;
		case SYSTEM_ENGINE_IDEL:
				if(Wifi_timer >= WIFI_ENGINE_IDEL_TIMEOUT){
					if(Wifi_status == WIFI_ON)
						Wifi_control(WIFI_OFF);
				}
				else{
					if(Wifi_status == WIFI_OFF)
						Wifi_control(WIFI_ON);
					Wifi_timer++;
				}
				break;
		case SYSTEM_ENGINE_START:
				if(Wifi_timer >= WIFI_ENGINE_START_TIMEOUT){
					if(Wifi_status == WIFI_ON)
						Wifi_control(WIFI_OFF);
				}
				else{
					//测试阶段发动机启动的时候不启动WIFI，如果原先启动的话则继续保持开启
					//2014/03/16
					//if(Wifi_status == WIFI_OFF)
					//	Wifi_control(WIFI_ON);
					Wifi_timer++;
				}
				break;
		case SYSTEM_ENGINE_STOP:
				if(Wifi_timer >= WIFI_ENGINE_STOP_TIMEOUT){
					if(Wifi_status == WIFI_ON)
						Wifi_control(WIFI_OFF);
					System_flag = SYSTEM_ENGINE_IDEL;
				}
				else{
					if(Wifi_status == WIFI_OFF)
						Wifi_control(WIFI_ON);
					Wifi_timer++;
				}			
				break;
		default:
				break;	
	}
}

/* RTC TICK Function On/Off */
void RTC_Enable(int status)
{
	if(status)
	{
		set_tick(80000000, RTC_1S);
	}
	else
	{
		dis_tick();
	}
}

/*Wifi开关控制函数*/
int Wifi_control(int flag)
{
	int delay = 10;
	if(flag == WIFI_ON){
		printf("Wifi On\n");
		//play sound of System Start
		saveSoundList(&sl, SOUND_WIFION_INDEX, Sound_status);
		Wifi_status = WIFI_ON;
		system("wifi on");
		sleep(1);
		system("ifconfig ra0 up");
		system("ifconfig ra0 192.168.111.1");
		system("dhcpd ra0 &");
		system("inetd &");
		Sound_status = SOUND_ON;
		saveSoundList(&sl, SOUND_WIFION_INDEX, Sound_status);
		if(wifion_sound_flag) delay = SOUND_WIFION_ALREADY;
		else delay = SOUND_WIFION_FIRST;
		while(delay--){
			printf("%d\n",delay);
			sleep(1);
		}
		Sound_status = SOUND_OFF;
		wifion_sound_flag = 1;
		
	}
	else{
		printf("Wifi Off\n");
		if(System_flag != SYSTEM_ENGINE_START){
			Sound_status = SOUND_ON;
			saveSoundList(&sl, SOUND_WIFIOFF_INDEX, Sound_status);
			if(wifioff_sound_flag) delay = SOUND_WIFIOFF_ALRAEDY;
			else delay= SOUND_WIFIOFF_FIRST;
			while(delay--){
				printf("%d\n",delay);
				sleep(1);
				}
			Sound_status = SOUND_OFF;
			wifioff_sound_flag = 1;
		}
		Wifi_status = WIFI_OFF;
		
		system("ifconfig ra0 down");
		system("killall dhcpd");
		system("killall inetd");
		system("wifi off");
	}
}


/*
 * 打印休眠触发位置信息
 */

void PrintSuspend(int flag)
{
	switch(flag)
	{
		case INITIAL:
			printf("@ Initial\n");
			break;
		case CHECKCONFIG:
			printf("@ Check Config\n");
			break;
		case CONNFAILED:
			printf(" @ Connect Failed\n");
			break;
		case NORMAL:
			printf(" @ Normal\n");
			break;
		default:
			printf("@ Default\n");
			break;
	}
}
  
/*
 * 进入休眠函数， 参数flag为休眠触发的位置
 */
void GotoSuspend(int flag)
{
		int delay = 0;
		int ret = 0;
		Sound_status = SOUND_ON;
		saveSoundList(&sl, SOUND_SLEEP_INDEX, Sound_status);
		if(suspend_sound_flag)  delay = SOUND_SUSPEND_ALREADY;
		else delay = SOUND_SUSPEND_FIRST;
		while(delay--)
		{
			sleep(1);
			printf("%d\n",delay);
		}
		system("./tmp/ftp/system/driver/obdif off");
		Sound_status = SOUND_OFF;
		//RTC_Enable(RTC_TICK_OFF);
		printf("System Power Down ");
		PrintSuspend(flag);
		//系统休眠问题没有解决前先用循环读取电瓶状态来替代
		while(!GetPowerStatus())	sleep(1);
		printf("System Resume ");
		PrintSuspend(flag);
		SetInterfaceGPIO();
		SetPowerGPIO();

		if(System_flag == SYSTEM_INITIAL)
		{
				Wifi_timer = 0;
				if(Wifi_status == WIFI_OFF)
						Wifi_control(WIFI_ON);
			}
		else
		{
			//如果不是在初始化状态中进入系统休眠，需要播放恢复音乐
			if(suspend_sound_flag)  delay = SOUND_RESUME_ALREADY;
			else delay = SOUND_RESUME_FIRST;
			Sound_status = SOUND_ON;
			saveSoundList(&sl, SOUND_ENGINEON_INDEX, Sound_status);
			while(delay--)
			{
				sleep(1);
				printf("%d\n",delay);
			}
			Sound_status = SOUND_OFF;
			
		}		
		//RTC_Enable(RTC_TICK_ON);
		suspend_sound_flag = 1;
		
}

/*主函数入口*/
int main(int argc, char *argv[])
{
	printf("**********Corefast Vehicle OBD Test APP **********\n");
	/* declare variables for STN1110 */ 
	int fd = -1;
    char buf[rxbuf_size];
	int baudrate = 9600,old_baud = 0;
	unsigned char arg_baud = 0;
	
	
	/* declare variables for Config File */
	FILE * cf;
	FILE * carfile;
	FILE * tempfile;
	
	unsigned char * tempfilename = malloc(256);
	unsigned char ConfigFileStatus = 0;
	unsigned char CarFileStatus = 0;
	unsigned char ConfigData[CONFIG_LENGTH];
	unsigned char checksum = 0;
    int flag = 0;
	unsigned char * cptr;

	unsigned char File_CID[CID_LEN];

	/* declare variables for General Purpose */
	int ret = 0;
	int index = 0;
	unsigned char alpha = 0;
	int retry = 0;
	
	/* declare variables for thread */
	//pthread_t th_gps, th_cal, th_sound;
	pthread_t th_cal, th_sound;
	int delay = 0;
	unsigned char pid[8];

	memset(&vi_real, 0x20, sizeof(struct vehicle_information));
	memset(&vs, 0, sizeof(struct vehicle_status));
	memset(&frp, 0, sizeof(struct fuel_rate_para));


	pthread_create(&th_sound, NULL, Start_Sound, 0);

    system("./tmp/ftp/system/driver/obdif off");
	/*-----Checking & Setting Baudrate for STN1110 -----*/
	fd = OpenDev(dev2);
	tcgetattr( fd,&old_options);
	if( argc > 1){
		baudrate = atoi(argv[1]);
		printf("OBD Baudrate Set: %d\n", baudrate);
	}
	for ( index = 0;  index < sizeof(name_arr) / sizeof(int);  index++){
		if  (baudrate == name_arr[index]){
			arg_baud = 1;
		 	}
	}
	if(!arg_baud){
		printf("Error Baudrate[%d, %s] Setting!\n", baudrate, argv[1]);
		quit(1,fd);
	}
	if (fd>0)
		set_speed(fd,baudrate);
	else{
		/* Do Sth. When STN1110 Failed */
		printf("Can't Open Serial Port [%s]!\n", dev2);
		quit(1,fd);
	}
	if (set_Parity(fd,8,1,'N')== FALSE){
		printf("Set Parity Error\n");
		quit(1,fd);
	}


	/* for stn1110 function selftest */
	if( argc > 2 && strcmp(argv[2], "debug") == 0)	selfTest(fd, RX_BUF_NAME);

	if( argc > 2 && strcmp(argv[2], "alpha") == 0) alpha = 1;
	
	index = 0;

	/* 检测STN1110波特率设定 */
	while(1){
		memset(RX_BUF_NAME, 0, rxbuf_size);
		//tcflush(fd, TCIOFLUSH);
		ret = AT_Get_Voltage(fd, RX_BUF_NAME);
		if(ret > 0){
			if( ret == 5){
				if(index == 0) old_baud = baudrate;
				processSTNreturn(RX_BUF_NAME, ret, GET_VOLTAGE, 0);	
				break;
			}
			else continue;
		}
		if(index > sizeof(name_arr) / sizeof(int)-1 ){
			printf("STN1110 Connect failed![%d/%d]\n", index, \
					sizeof(name_arr) / sizeof(int));
			quit(1,fd);
		}
		else{	
			old_baud = name_arr[index];
			set_speed(fd,name_arr[index]);
			index++;
		}
		printf("Try Baudrate->%d\n", old_baud);
		usleep(200000);
	}

	/* Change Baudrate if necessary */
	if(baudrate != old_baud)	{
		
		printf("Old Buadrate is %d\n", old_baud);

		//ret = ST_Set_BRD_Software(fd, buf, baudrate);
		tcflush(fd, TCIOFLUSH);
		ret = ST_Set_BRD_Terminal(fd, RX_BUF_NAME, baudrate);		
		processSTNreturn(RX_BUF_NAME, ret, SET_BRD_Terminal, 0);	
		
		set_speed(fd,baudrate);
				
		while(1){
			memset(RX_BUF_NAME, 0, rxbuf_size);	
			tcflush(fd, TCIOFLUSH);
			/* Save STN1110 BRT PARAMETER */
			ret = ST_Set_BRtoNVM(fd, RX_BUF_NAME);
			processSTNreturn(RX_BUF_NAME, ret, SET_BRTONVM, 0);
			if(ret >= 2 && strcmp(RX_BUF_NAME, "OK") == 0){		
				break;
			}
			usleep(200000);
		}

		printf("New Baudrate is %d. \n", baudrate);
	}
	else	printf("Device Baudrate: %d.\n",baudrate);

	/* if the 2nd parameter is test baud, program is finished here */ 
	if( argc > 2 && strcmp(argv[2], "testbaud") == 0)	quit(1,fd);

	/* for stn1110 reset here */
		tcflush(fd, TCIOFLUSH);
		ret = AT_Reset(fd, RX_BUF_NAME);
		processSTNreturn(RX_BUF_NAME, ret, AT_RESET, 0);
		usleep(500000);
		
	if( argc > 2 && strcmp(argv[2], "reset") == 0){
		sleep(1);
		quit(0,fd);
	}
	

	/* 设定RTC 中断 */
	open_rtc(getpid());
	RTC_Enable(RTC_TICK_ON);

	/*================ 	System Start 	===============*/	

	init_Trip(&tstatus);
	init_CAL(&cdata, &mdata, &sdata);
	init_OBD_DATA(&odata);
	printf("OBD data initial Finished.\n");

	pthread_create(&th_cal, NULL, CalCarData, 0);
	printf("Thread Calculation Created. \n");

	//Set obd protocol to auto search mode to auto identify the obd protocol
	printf("SET OBD  Protocol of Vehicle as Auto Mode\n");
	index = 0;

	/*  设置OBD协议为自动协议 */
	while(1)	{
		memset(RX_BUF_NAME, 0, rxbuf_size);
		tcflush(fd, TCIOFLUSH);
		ret = AT_Try_ProtocolAuto(fd, RX_BUF_NAME, 0);
		if( alpha == 1) {
			processSTNreturn(RX_BUF_NAME, ret, TRY_AUTO_PROTOCOL, 0);
		}
		if(strcmp(RX_BUF_NAME, "OK") == 0) break;
		index++;
		if(index > STN_ERROR_TRIES){
			/*  if necessary to add alarm here */
			printf("Try auto protocol Error!\n");
			quit(0,fd);
		}
		usleep(200000);
	}	

checkconfig:
	
	/*------------ 上电检查 Config.in，在系统成功运行后是不会执行到这里的------------*/
	cf = fopen(CONFIG_FILE, "r");
	carfile = fopen(CONFIG_CAR, "r");

	/* 当缺少配置文件或备份文件的时候，需要删除存在的文件并重新初始化配置
	 *	配置完成后重新启动系统
	 * 当设备被拔下后，则会重新检测
	 */ 
	if( cf == NULL || carfile == NULL ){	//缺少配置文件，系统要初始化
		printf("Config File or Car File not existed.\n");
		printf("System is asked to Initial!\n");
		/*	remove or rename existed config file*/
		if(cf != NULL){
			fclose(cf);
			if( alpha == 1)
			{
				/* Change name to backup when run in alpha mode */
				index = 0;
					
				while(1){
					sprintf(tempfilename,"%s.%d", \
											CONFIG_FILE, index);
					if( (tempfile = fopen(tempfilename, "r") ) == NULL ){
						rename(CONFIG_FILE, tempfilename);
						break;
					}
					index++;
				}
			}
			else{
				remove(CONFIG_FILE);
			}
			ConfigFileStatus = 0;
		}
		/*	remove or rename existed car file*/
		if(carfile != NULL){
			fclose(carfile);
			if( alpha == 1)
			{
				/* Change name to backup when run in alpha mode */
				index = 0;
					
				while(1){
					sprintf(tempfilename,"%s.%d", \
											CONFIG_CAR, index);
					if( (tempfile = fopen(tempfilename, "r") ) == NULL ){
						rename(CONFIG_CAR, tempfilename);
						break;
					}
					index++;
				}
			}
			else{
				remove(CONFIG_CAR);
			}
			CarFileStatus = 0;
		}

		/* 开始初始化 */
		memset(ConfigData, 0x20, CONFIG_LENGTH);

		index = 0;
		printf("Waitting for OBD Start \n");
		SetInterfaceGPIO();
		while(1){	//Waitting for OBD Start 
			/* Wait for Connection with Vehicle */
			while(GetInterfaceStatus()==0)
			{
				sleep(1);
				index++;
				printf("ECU is powered off\n");
				if(index > VEHICLE_INITIAL_WAITTING)
				{
					/* Let STN1110 Sleep */
					while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
						sleep(1);		
					GotoSuspend(INITIAL);
					index = 0;					
				}
			}
			
			system("./tmp/ftp/system/driver/obdif on");
			memset(RX_BUF_NAME, 0, rxbuf_size);
			/* 如果此处OBD信号接口有电压尝试连接，超过连接尝试次数, 则放弃尝试，
			 * 播放ENGINE OFF音乐
			 *
			 */
			if(index >=VEHICLE_CONN_TRIES){
				/* 当等于尝试次数的时候播放音乐并将index加一，即只播放一次*/
				if(index == VEHICLE_CONN_TRIES)
				{
					Sound_status = SOUND_ON;
					saveSoundList(&sl, SOUND_ENGINEOFF_INDEX, Sound_status);
					delay = 20;
					while(delay--)
					{
						printf("%d\n", delay);
						sleep(1);
					}
					index++;
				}
				printf("OBD Connect Overtime \n");
				
			}
			else
			{
				if( STN_Check_Connect(fd, RX_BUF_NAME, alpha) ) break;
				/* 尝试失败播放 ENGINE IDLE音乐 */
				Sound_status = SOUND_ON;
				saveSoundList(&sl, SOUND_LINK_INDEX, Sound_status);
				delay = 5;
				while(delay--)
				{
					printf("%d\n",delay);
					sleep(1);
				}
				Sound_status = SOUND_OFF;			
				index++;
			}
		}

		/* Vehicle ECU Start Work */
		GetDataFromVehicle(fd, ConfigData, &vi_real, alpha);
		/* Save ConfigData to File */
		cf = fopen(CONFIG_FILE, "w+");
		carfile = fopen(CONFIG_CAR, "w+");		
		SaveFile(cf, ConfigData, CONFIG_LENGTH);
		SaveFile(carfile, ConfigData, CONFIG_LENGTH);
		fclose(cf);
		fclose(carfile);		
		if(alpha) Print_Datas(ConfigData, CONFIG_LENGTH, "Config Data:");	
		vs.ConfigFileOK = 3;
		/* 等待文件写入SD卡，5秒后重新启动 */
		printf("System now restarting\n");
		index = 6;
		while(index--){
			printf("%d\n", index);
			sleep(1);	}
		system("reboot");
		//goto checkconfig;
	}
	else{	// 配置文件已存在
		if(GetFileLength(cf) != 2 * CONFIG_LENGTH)	{ // 配置文件已存在，但发生损坏
			printf("Config File is Damaged!\n");
			// Do Something to repair Config File
			printf("Repairing Config File...\n");
			fclose(cf);
			/* 备份的配置文件也发生损坏 */
			if(GetFileLength(carfile) != 2* CONFIG_LENGTH){
				printf("Backup of Config File is also Damaged too!\n");
				fclose(carfile);
				if( alpha == 1){
					/* 在调试模式下，备份损坏的配置文件 */
					index = 0;	//这个index用于备份损坏配置文件的编号	
					while(1){
						sprintf(tempfilename,"%s.%d", 	CONFIG_FILE, index);
						if( (tempfile = fopen(tempfilename, "r") ) == NULL ){
								rename(CONFIG_FILE, tempfilename);
								break;}
							index++;}}
				else{
						/* 在正常运行模式下， 直接删除配置文件，系统重起进入初始化配置*/
						remove(CONFIG_FILE);}
				//System reboot to Initial Device
				printf("System now restarting @ Config File Damaged\n");
				index = 6;
				while(index--){
						printf("%d\n", index);
						sleep(1);	}
				system("reboot");
					//goto checkconfig;
					}
			/* 备份文件是好的情况下 */
			else{
				if( alpha == 1){	//调试模式下备份原先损坏的配置文件
					/* Change name to backup when run in alpha mode */
					index = 0;			
					while(1){
						sprintf(tempfilename,"%s.%d", CONFIG_FILE, index);
						if( (tempfile = fopen(tempfilename, "r") ) == NULL ){
								rename(CONFIG_FILE, tempfilename);
								break;}
						index++;}}
				else /* 正常运行模式下， 删除损坏的配置文件 */
					remove(CONFIG_FILE);
						
		/*<-----------------------------需要考虑------------------------------------>*/
				/* 恢复配置文件，记录清零 */
				/* Save ConfigData to File */
				cf = fopen(CONFIG_FILE, "w+");
				memset(ConfigData, 0x20, CONFIG_LENGTH);
				GetDataFromFile(carfile, ConfigData, CONFIG_LENGTH);
				/* Set Last Record Number to 0 */
				AddLastRecordNumber(ConfigData, 0);
		/*<------------------------------------------------------------------------>*/				
				/* Save Config Data Buffer to File */
				SaveFile(cf, ConfigData, CONFIG_LENGTH);
				fclose(cf);
				fclose(carfile);
				//System reboot to restart Running
				printf("System now restarting\n");
				index = 6;
				while(index--){
						printf("%d\n", index);
						sleep(1);}					
				system("reboot");
				//goto checkconfig;
				}
		}

		/*----Checking Device is for this Car ----*/
		if(alpha) printf("System Checking if Config file is Valid....\n");
		/*--------------------检查Config文件是否和车辆配对---------------------*/
		if(alpha) printf("Get Vehicle Information from Config File\n");
		memset(ConfigData, 0x20, CONFIG_LENGTH);
		/* Get Vehicle data from Config File */
		GetDataFromFile(cf, ConfigData, CONFIG_LENGTH);
		fclose(cf);
		fclose(carfile);
		
		/* Update Last Trip File Number */
		updateFileIndex(&sdata, GetLastRecordNumFromBuffer(ConfigData) + 1);

		/* Get CID which stored in Config File */
		memset(File_CID, 0x20, CID_LEN);
		cptr = ConfigData + CONFIG_CID_ADDR;
		memcpy(File_CID, cptr, CID_LEN);
		//if(alpha) Print_Datas(ConfigData, CONFIG_LENGTH, "Config Data:");	

		/* Start to Get Config Data from Vehicle */
		memset(ConfigData, 0x20, CONFIG_LENGTH);	
		printf("Get Information from Vehicle's OBD Port\n");
		index = 0;
		/* Wait for Connection with Vehicle */
		SetInterfaceGPIO();
		
		while(GetInterfaceStatus()==0){ /* 等待ECU打开 */
			index++;
			sleep(1);
			printf("ECU is powered off\n");
			if(index > VEHICLE_INITIAL_WAITTING)	{ // 等待超时
				/* Let STN1110 Sleep */
				while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
					sleep(1);		
				GotoSuspend(CHECKCONFIG);			
				index = 0;}
			}
		
		system("./tmp/ftp/system/driver/obdif on");

		while(1){	/* 尝试连接STN1110 */				
			if(STN_sleep == 0)	{
				memset(RX_BUF_NAME, 0, rxbuf_size);
				if(alpha) printf("Try [%d] Time(s): ",index);	
				if(STN_Check_Connect(fd, RX_BUF_NAME, alpha) ) break;
				index++;		
				if(index > VEHICLE_CONN_TRIES){
					/* Do Something when STN Connect Vehicle Overtime */
					printf("Connect Overtime \n");
					memset(RX_BUF_NAME, 0, rxbuf_size);
					while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
						sleep(1);
					if(alpha) printf("STN Enter Sleep Mode(%d) %s\n",ret,RX_BUF_NAME);
					STN_sleep = 1;}
			}
			else{	//连接超时休眠		
				system("./tmp/ftp/system/driver/obdif off");
				/* Let STN1110 Sleep */
				while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
					sleep(1);		
				GotoSuspend(CONNFAILED);
				index = 0;
				STN_sleep = 0;		
				system("./tmp/ftp/system/driver/obdif on");
			}
			sleep(1); //等待声音播放完毕
		}

		/* Vehicle ECU Start Work */
		tcflush(fd, TCIOFLUSH);
		GetDataFromVehicle(fd, ConfigData, &vi_real, alpha);

		if(alpha)
		{
			Print_Datas(ConfigData, CONFIG_LENGTH, "Config Data:");	
			printf("VCID: ");
			for(index = 0; index < CID_LEN; index++) printf("%02X ", vi_real.CID[index]);
			printf("\n");
			printf("FCID: ");
			for(index = 0; index < CID_LEN; index++) printf("%02X ", File_CID[index]);
			printf("\n");
		}	
		/* Compare CID from Vehicle with which from Config File */
		for(index = 0; index < CID_LEN; index++)	{
			if(vi_real.CID[index] != File_CID[index]){
				/* Do Someting for Device is not compare with Vehicle */
				printf("This devcie is not for your vehicle!\n");
				close(fd);			
				while(1){ // Loop Play SOUND_BRK_INDEX, Need Replug
					Sound_status = SOUND_ON;
					saveSoundList(&sl, SOUND_BRK_INDEX, Sound_status);
					delay = 10;
					while(delay--)
					{
						printf("%d\n", delay);
						sleep(1);
					}
				}		
			}
		}
		/*--------------------检查设备配对完毕---------------------*/

		
		/*------------------计算方式配置 -----------------------*/
		/* Clear Parameter */
		memset(&frp, 0, sizeof(struct fuel_rate_para));
		frp.CONSTANT = FRP_CONSTANT;
		frp.VE = FRP_VE;
		frp.ED = FRP_ED;
		frp.R = FRP_R;
		frp.MAIR = FRP_MAIR;
		frp.AFR = FRP_AFR;
		frp.FUELP = FRP_FUELP;
		
		/* Setting Formulate for calculation of Fuel rate */
		if(alpha) printf("Fuelrate Calculation Formulate [%2X][%2X] Set -> ", \
								vi_real.gas_cal, vi_real.fuel_cal[0]);
								
		if(vi_real.gas_cal == 0x5E)
		{
				vcmd.GetFR = Cal_FR_From_EFR;
				if(alpha) printf("Cal_FR_From_EFR	\n");
		}		
		else
		{
			if(vi_real.fuel_cal[0] == 0x0B)
			{
				vcmd.GetFR = Cal_FR_From_MAP;
				if(alpha) printf("Cal_FR_From_MAP \n");
			}			
			else if(vi_real.fuel_cal[0] == 0x10)
			{
				vcmd.GetFR = Cal_FR_From_MAF;
				if(alpha) printf("Cal_FR_From_MAF \n");			
			}

		}
		/*------------------计算方式配置完毕 -----------------------*/
		Sound_status = SOUND_ON;
		//play sound of System Start
		saveSoundList(&sl, SOUND_START_INDEX, Sound_status);
		delay = 15;
		while(delay--)
		{
				printf("%d\n",delay);
				sleep(1);
			}
		Sound_status = SOUND_OFF;
		
		/* Get Initial Battery Voltage */
		memset(RX_BUF_NAME, 0, rxbuf_size);
		tcflush(fd, TCIOFLUSH);
		ret = AT_Get_Voltage(fd, buf);
		if(ret > 4 && buf[4] == 'V' && buf[2] == '.'){
			cptr = buf;				
			vs.Voltage = HexToChar(buf[0])*100 + HexToChar(buf[1])*10 \
								+ HexToChar(buf[3]);
			wBAT = vs.Voltage;
			printf("Battery Voltage : %d.%d V \n", vs.Voltage /10, vs.Voltage % 10); 
		}//if

		Wifi_timer = 0;								//WIFI 计数器清零
		System_flag = SYSTEM_ENGINE_IDEL;		//系统状态设为引擎闲置
		
		/* System Running */		
		while(1)
		{
			memset(RX_BUF_NAME, 0, rxbuf_size);
				
			SetPowerGPIO();
			if(!GetPowerStatus()){
				/* Get Bat Voltage When ECU OFF */
				ret = AT_Get_Voltage(fd, buf);
				if(ret > 4 && buf[4] == 'V' && buf[2] == '.'){
					cptr = buf;				
					vs.Voltage = HexToChar(buf[0])*100 + HexToChar(buf[1])*10 \
										+ HexToChar(buf[3]);
					wBAT = vs.Voltage;
					printf("Battery Voltage : %d.%d V \n", vs.Voltage /10, vs.Voltage % 10); 
				}//if
				//play sound of ENGINE Start
				//saveSoundList(&sl, SOUND_ENGINEIDLE_INDEX, Sound_status);
				index++;
				printf("ECU is powered off\n");
				if(index > VEHICLE_IS_STOP)
				{
					/* Let STN1110 Sleep */
					while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
						sleep(1);		
					GotoSuspend(NORMAL);
					system("./tmp/ftp/system/driver/obdif on");
					
					/*  设置OBD协议为自动协议 */
					flag = 1;
					index = 0;
					while(flag)	{
						memset(RX_BUF_NAME, 0, rxbuf_size);
						tcflush(fd, TCIOFLUSH);
						ret = AT_Try_ProtocolAuto(fd, RX_BUF_NAME, 0);
						if( alpha == 1) {
							processSTNreturn(RX_BUF_NAME, ret, TRY_AUTO_PROTOCOL, 0);
						}
						if(strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
						
						if(index > STN_ERROR_TRIES){
							/*  if necessary to add alarm here */
							printf("Try auto protocol Error![%d]\n",index);
							Wifi_control(WIFI_OFF);
							quit(0,fd);
						}
						index++;
						usleep(200000);
					}	
					

					flag = 1;
					index = 0;
					while(flag){	/* 尝试连接STN1110 */				
						if(STN_sleep == 0)	{
							memset(RX_BUF_NAME, 0, rxbuf_size);
							if(alpha) printf("Try [%d] Time(s): ",index);	
	
							if(index > VEHICLE_CONN_TRIES){
								/* Do Something when STN Connect Vehicle Overtime */
								printf("Connect Overtime \n");
								memset(RX_BUF_NAME, 0, rxbuf_size);
								while(ST_EnterSleep(fd, RX_BUF_NAME,0) != 2 || strcmp(RX_BUF_NAME, "OK") != 0) 
									sleep(1);
								if(alpha) printf("STN Enter Sleep Mode(%d) %s\n",ret,RX_BUF_NAME);
								STN_sleep = 1;}
								
							if(STN_Check_Connect(fd, RX_BUF_NAME, alpha) ) flag = 0;
							index++;						
						}
						else{	//连接超时休眠		
								index = 0;
						}
						sleep(1); //等待声音播放完毕
					}

					flag = 1;				
					while(flag){
						memset(RX_BUF_NAME, 0, rxbuf_size);
						ret = AT_Set_Header(fd, RX_BUF_NAME, 1);
						//printf("ret:%d \n", ret);
							if( alpha ) {
								processSTNreturn(RX_BUF_NAME, ret, SET_HEADER, 1);
							}
						if(ret == 2 && strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
						usleep(10000);
					}
					printf("Turn Header On\n");
				
					if(vi_real.protocol_num > 5){
						while(CAN_Judge_ECUS(fd,RX_BUF_NAME, 1) == 0)
							sleep(1);
					}
					else
					{
						flag = 1;
						index = 0;
						while(flag){
							memset(RX_BUF_NAME, 0, rxbuf_size);
							ret = AT_Set_HeaderValue_B(fd, RX_BUF_NAME, 0xE4, 0x10, 0xF1);
							if(strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
							if(index > STN_ERROR_TRIES){
								/*  if necessary to add alarm here */
								printf("Try setting KWP Header Error[%d]\n",index);
								Wifi_control(WIFI_OFF);
								quit(0,fd);
							}
							index++;
							usleep(200000);
						}
					}
					printf("Engine Address Filtting OK\n");
					
					index = 0;							
				}
				sleep(1);
				continue;		
			}

			/* Start Read RPM When Engine Start */
			//if(vs.Voltage > cBAT + 8) frp.rpm = vcmd.GetRPM(fd, RX_BUF_NAME, alpha, &vs);
			//else frp.rpm = 0;
			/* Calculation for Fuel Rate of Vehicle */
			printf("Engine Start\n");
			retry = 0; //for connect retry count
			Sound_status = SOUND_OFF;
				//play sound of ENGINE Start
				//saveSoundList(&sl, SOUND_ENGINEON_INDEX, Sound_status);
			Wifi_timer = 0;								//WIFI 计数器清零
			System_flag = SYSTEM_ENGINE_START;		//系统状态设为引擎闲置
			EngineStart();
			index = 0;
			frp.rpm = 700;

			/*启动时油箱存量*/
			if(vi_real.POI == 0x2F){
				//待测试
				ret = vcmd.GetFUELLV(fd, RX_BUF_NAME, alpha, &vs);
				if(ret != STN_NONE_RETURN){
					updateSummaryFuelLV(&sdata, ret);
				}
			}
			else{
					updateSummaryLastFuelLV(&sdata, 0xFF);}
			force_stop = 0;
			while((GetPowerStatus() || frp.rpm > 100) && force_stop ==0)	{ //电压超过设定电压或转速大于100判断为持续发动机运行			
					if(frp.rpm > 100){
						//if(alpha) 
							printf("[No. %8d]\n", ++index);
						
						if(vi_real.protocol_type == PROTOCOL_TYPE_KWP){ /* KWP Protocol Process */
							/* Get Vehicle Data for Calculation of Fuel Rate */
							if(vi_real.gas_cal == 0x5E){
								//tcflush(fd, TCIOFLUSH);
								ret = vcmd.GetEFR(fd, RX_BUF_NAME, alpha, &vs);
								if(ret != STN_NONE_RETURN) frp.fuelrate = ret;
								usleep(OBD_CMD_DELAY);
							}
							else{						
								if(vi_real.fuel_cal[0] == 0x0B ){	
									ret = 	vcmd.GetMAP(fd, RX_BUF_NAME, alpha, &vs);	// 0B			--MAP MAF 1							
									if(ret != STN_NONE_RETURN) frp.map = ret;
									
									usleep(OBD_CMD_DELAY);
									if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
									
									ret = vcmd.GetIAT(fd, RX_BUF_NAME, alpha, &vs);	// 0F			--IAT MAF 2
									if(ret != STN_NONE_RETURN) frp.iat = ret;
									usleep(OBD_CMD_DELAY);
									if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
								}
								else if( vi_real.fuel_cal[0] == 0x10){
									ret = vcmd.GetMAF(fd, RX_BUF_NAME, alpha, &vs);	// 10			--MAF MAP 1
									if(ret != STN_NONE_RETURN) frp.maf = ret;
									usleep(OBD_CMD_DELAY);
									if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
								}
								ret = vcmd.GetSTFT(fd, RX_BUF_NAME, alpha, &vs);	// 06			--STFT FR 1
								if(ret != STN_NONE_RETURN) frp.stft = ret;
								usleep(OBD_CMD_DELAY);
								if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
								
								ret = vcmd.GetLTFT(fd, RX_BUF_NAME, alpha, &vs);	// 07			--LTFT FR 2
								if(ret != STN_NONE_RETURN) frp.ltft = ret;
								usleep(OBD_CMD_DELAY);
								if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);		
							}
						
							/* Get Vehicle Speed */
							ret = vcmd.GetSPEED(fd, RX_BUF_NAME, alpha, &vs);		// 0D			-- SPD 
							if(ret != STN_NONE_RETURN) frp.speed = ret;
							updateOBDDATA(&odata, frp.speed, OBD_DATA_SPD);
							usleep(OBD_CMD_DELAY);
							if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
						
							/* Get Engine RPM */
							
						//	ret = vcmd.GetRPM(fd, RX_BUF_NAME, alpha, &vs);			// 0C			-- RPM
						//	if(ret != STN_NONE_RETURN) frp.rpm = ret;
							
							frp.rpm  = vcmd.GetRPM(fd, RX_BUF_NAME, alpha, &vs);
							if(frp.rpm > 0 ) updateOBDDATA(&odata, frp.rpm, OBD_DATA_RPM);		
							usleep(OBD_CMD_DELAY);
							if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
							
							if( argc > 3 && strcmp(argv[3], "FRP") == 0){
								printf("RPM: %4d, SPEED: %4d\nMAP: %4d, IAT: %4d\n, MAF: %4d, STFT: %4d, LTFT: %4d\n", \
										 frp.rpm, frp.speed, frp.map, frp.iat, frp.maf, frp.stft, frp.ltft);

								if(vi_real.gas_cal == 0x5E || vi_real.fuel_cal[0] != 0x20)
									printf("Fuel Rate: %8d mL/s\n", frp.fuelrate);
							}
							
							/* Get Vehicle Data for Calculation of Drive Behavior */
							/* Get Engine Load */
							ret = vcmd.GetENGINELOAD(fd, RX_BUF_NAME, alpha, &vs);
							if(ret != STN_NONE_RETURN)
								updateOBDDATA(&odata, ret, OBD_DATA_EL);			// 04			--	Engine Load
							usleep(OBD_CMD_DELAY);
							if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);

							/* Get Cool Temperature */
							ret = vcmd.GetCOOLTEMP(fd, RX_BUF_NAME, alpha, &vs);
							if(ret != STN_NONE_RETURN)						
								updateOBDDATA(&odata, ret, OBD_DATA_CT);			// 05			-- CT
							usleep(OBD_CMD_DELAY);
							if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);

							/* Get Pad Postion */
							ret = vcmd.GetPADPOS(fd, RX_BUF_NAME, alpha, &vs);
							if(ret != STN_NONE_RETURN)
								updateOBDDATA(&odata, ret, OBD_DATA_PAD);		// 11			--	PP
							usleep(OBD_CMD_DELAY);
							if(vi_real.protocol_num == 3) usleep(OBD_CMD_DELAY1);
						}
						
						
						else if(vi_real.protocol_type == PROTOCOL_TYPE_CAN){ /* CAN Protocol Process */					
							if(vi_real.gas_cal == 0x5E)
							{
								if(vcmd.GetMultiEFR(fd, RX_BUF_NAME, alpha, &vs, &frp) > 0)
								{
									updateOBDDATA(&odata, (int)(vs.Engine_Load * 100 / 255), OBD_DATA_EL);
									updateOBDDATA(&odata, (int)(vs.Cool_Temp), OBD_DATA_CT);
									updateOBDDATA(&odata, (int)(vs.Pad_Position * 100 / 255), OBD_DATA_PAD);
									updateOBDDATA(&odata, frp.speed, OBD_DATA_SPD);
									updateOBDDATA(&odata, frp.rpm, OBD_DATA_RPM);
								}				
							}
							else{
								/* Get Vehicle Data for Calculation of Fuel Rate */
								if(vi_real.fuel_cal[0] == 0x0B ){
									if(vcmd.GetMultiMAP(fd, RX_BUF_NAME, alpha, &vs, &frp) > 0)
									{
										updateOBDDATA(&odata, frp.speed, OBD_DATA_SPD);
										updateOBDDATA(&odata, frp.rpm, OBD_DATA_RPM);
										printf("Speed: %d, RPM: %d, FuelRate: %d \n", frp.speed, frp.rpm,frp.fuelrate);
									    printf("STFT: %d, LTFT: %d, MAP: %d\n", frp.stft, frp.ltft,frp.map);
									    retry = 0;
									}
									else
									{
										//force_stop = 1;
										frp.rpm = STN_NONE_RETURN;
										continue;
										}
								}
								else if( vi_real.fuel_cal[0] == 0x10){
									if(vcmd.GetMultiMAF(fd, RX_BUF_NAME, alpha, &vs, &frp) > 0)
									{
										updateOBDDATA(&odata, frp.speed, OBD_DATA_SPD);
										updateOBDDATA(&odata, frp.rpm, OBD_DATA_RPM);
									    printf("Speed: %d, RPM: %d, FuelRate: %d \n", frp.speed, frp.rpm,frp.fuelrate);
									    printf("STFT: %d, LTFT: %d, MAF: %d\n", frp.stft, frp.ltft,frp.maf);
									    retry = 0;
									}
									else
									{
										//force_stop = 1;
										frp.rpm = STN_NONE_RETURN;
										continue;
										}
								}
								/* Get Vehicle Data for Calculation of Drive Behavior */
								if(vcmd.GetMultiDriveBehavior(fd,RX_BUF_NAME, alpha, &vs) > 0)
								{
									updateOBDDATA(&odata, (int)(vs.Engine_Load * 100 / 255), OBD_DATA_EL);
									updateOBDDATA(&odata, (int)(vs.Cool_Temp), OBD_DATA_CT);
									updateOBDDATA(&odata, (int)(vs.Pad_Position * 100 / 255), OBD_DATA_PAD);
									retry = 0;
								}
								else
								{
									//force_stop = 1;
									frp.rpm = STN_NONE_RETURN;
									continue;
								}
								
							}
						}

						if(vi_real.POI == 0x2F){
								//待测试
								vcmd.GetFUELLV(fd, RX_BUF_NAME, alpha, &vs);}
						else{
							vs.Fuel_LV = 0xFF;
						}
					}
					else if (frp.rpm < 0){//frp.rpm < 0 
						ret = AT_Reset(fd, RX_BUF_NAME);
						processSTNreturn(RX_BUF_NAME, ret, AT_RESET, 0);
						usleep(500000);

						/*  设置OBD协议为自动协议 */
						flag = 1;
						index = 0;
						while(flag){
							memset(RX_BUF_NAME, 0, rxbuf_size);
							tcflush(fd, TCIOFLUSH);
							ret = AT_Try_ProtocolAuto(fd, RX_BUF_NAME, 0);
							if( alpha == 1) {
								processSTNreturn(RX_BUF_NAME, ret, TRY_AUTO_PROTOCOL, 0);
							}
							if(strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
							
							if(index > STN_ERROR_TRIES){
								/*  if necessary to add alarm here */
								printf("Try auto protocol Error![%d]\n",index);
								Wifi_control(WIFI_OFF);
								quit(0,fd);
							}
							index++;
							usleep(200000);
						}	
							

						memset(RX_BUF_NAME, 0, rxbuf_size);									
						if(!STN_Check_Connect(fd, RX_BUF_NAME, alpha)) continue;
	
						flag = 1;
						index = 0;			
						while(flag){
							memset(RX_BUF_NAME, 0, rxbuf_size);
							ret = AT_Set_Header(fd, RX_BUF_NAME, 1);
							//printf("ret:%d \n", ret);
								if( alpha ) {
									processSTNreturn(RX_BUF_NAME, ret, SET_HEADER, 1);
								}
							if(ret == 2 && strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
							usleep(10000);
						}
						printf("Turn Header On\n");
						
						if(vi_real.protocol_num > 5){
							while(CAN_Judge_ECUS(fd,RX_BUF_NAME, 1) == 0)
								sleep(1);
						}
						else
						{
							flag = 0;
							index = 0;
							while(flag){
								memset(RX_BUF_NAME, 0, rxbuf_size);
								ret = AT_Set_HeaderValue_B(fd, RX_BUF_NAME, 0xE4, 0x10, 0xF1);
								if(strncmp(RX_BUF_NAME, "OK", 2) == 0) flag = 0;
								if(index > STN_ERROR_TRIES){
									/*  if necessary to add alarm here */
									printf("Try setting KWP Header Error[%d]\n",index);
									Wifi_control(WIFI_OFF);
									quit(0,fd);
								}
								index++;
								usleep(200000);
							}
						}
						printf("Engine Address Filtting OK\n");
						frp.rpm = 700;
						retry++;
						
						if(retry > 3 && vi_real.protocol_num > 3){
							/* Do Something when STN Connect Vehicle Overtime */
							printf("Connect GIVE up \n");
							frp.rpm = 0;
							//break;
						}
						if(retry > 10 && vi_real.protocol_num == 3){
							/* Do Something when STN Connect Vehicle Overtime */
							printf("A3 Connect GIVE up \n");
							frp.rpm = 0;
							//break;
						}
						
						usleep(200000);
					}	//while(frp.rpm  > 100 || frp.rpm < 0)
					else{
						break;
					}
				}//if(frp.rmp > 100)

			printf("Engine Stop\n");
			if(vi_real.dist_cal[0] == 0x21){
					tcflush(fd, TCIOFLUSH);
					ret = vcmd.GetDIST21(fd, RX_BUF_NAME, alpha, &vs);	 // 21
					if(ret != STN_NONE_RETURN){

					}
			}
			/*停止时油箱存量*/
			if(vi_real.POI == 0x2F){
					//待测试
					ret = vcmd.GetFUELLV(fd, RX_BUF_NAME, alpha, &vs);
					if(ret != STN_NONE_RETURN){
						updateSummaryLastFuelLV(&sdata, ret);
					}
			}
			else{
					updateSummaryLastFuelLV(&sdata, 0xFF);}
					
			EngineStop();
			//play sound of ENGINE Stop
			//saveSoundList(&sl, SOUND_ENGINEOFF_INDEX, Sound_status);
			Sound_status = SOUND_ON;
			Wifi_timer = 0;								//WIFI 计数器清零
			System_flag = SYSTEM_ENGINE_STOP;		//系统状态设为引擎闲置
			index = 15;
			while(index--){
				printf("Engine Stop Delay %d\n", index);
				sleep(1);
			}
			index = 0;	
		}// while System Running
			
	}//else
		
	close(fd);
	printf("Application is Ready to Exit!\n");
	index = 6;
	while(index--){
		printf("%d\n", index);
		sleep(1);	}
	return 0;
}
